% eye in hand calibrator
tcp2world = load('myfile.txt'); % load robot TCP pose reference to robot base (or world) frame
grid2camera = load('chessboardPose.txt'); %load grid (chessboard) pose reference to camera frame

[r, c] = size(tcp2world);

tcp2world2 = zeros(4,4,r); % rewrite  pose to (4x4, viewNumber)

for i=1:r
    for j=1:4
        if j==4
            tcp2world2(j, 1:4, i) = [0,0,0,1];
            continue;
        end
        tcp2world2(j, 1:4, i) = tcp2world(i, ((j-1)*4+1):(j*4));
    end
end

grid2camera2 = zeros(4,4,r); % rewrite  pose to (4x4, viewNumber)
for i=1:r
    for j=1:4
        if j==4
            grid2camera2(j, 1:4, i) = [0,0,0,1];
            continue;
        end
        grid2camera2(j, 1:4, i) = grid2camera(i, ((j-1)*4+1):(j*4));
    end
end

% calculate the camera pose reference to the TCP frame, and the error
[cam2tcp, err] = TSAIleastSquareCalibration(tcp2world2, grid2camera2)


